#ifndef UTILITY_H_
#define UTILITY_H_
#include <chrono>
#include <cmath>
#include <limits>
#include <string>

namespace autodrive {
namespace perception {
class Timer {
 public:
  explicit Timer() noexcept
      : m_begin_(std::chrono::high_resolution_clock::now()) {};
  ~Timer() = default;
  Timer(Timer const&) noexcept = default;
  Timer& operator=(Timer const&) noexcept = default;
  Timer(Timer&&) noexcept = default;
  Timer& operator=(Timer&&) noexcept = default;

  template <typename Duration = std::chrono::microseconds>
  int64_t elapsed() const {
    return std::chrono::duration_cast<Duration>(
               std::chrono::high_resolution_clock::now() - m_begin_)
        .count();
  }

 private:
  std::chrono::time_point<std::chrono::high_resolution_clock> m_begin_;
};

inline std::string to_green(std::string str) noexcept {
  return "\033[1;32m" + str + "\033[0m";
}

inline std::string to_red(std::string str) noexcept {
  return "\033[1;5;31m" + str + "\033[0m";
}

inline std::string to_yellow(std::string str) noexcept {
  return "\033[1;5;33m" + str + "\033[0m";
}

constexpr long long int operator"" _GiB(unsigned long long val) {
  return val * (1 << 30);
}
constexpr long long int operator"" _MiB(unsigned long long val) {
  return val * (1 << 20);
}
constexpr long long int operator"" _KiB(unsigned long long val) {
  return val * (1 << 10);
}

/// @brief judge d1>d2?
/// @tparam T type, float or double
/// @param d1 data 1
/// @param d2 data 2
/// @return d1>d2 return true, otherwise false
template <typename T>
bool is_greater(T const& d1, T const& d2) noexcept {
  static_assert(std::is_floating_point_v<T>,
                "data type must be float or double");
  bool cmp1{std::abs(d1 - d2) > std::numeric_limits<T>::epsilon()};
  bool cmp2{d1 > d2};
  return cmp1 && cmp2;
}

/// @brief clip data in range
/// @tparam T output data type
/// @param data data to clip
/// @return clipped data
template <typename T>
T clip(double data) {
  static_assert(std::is_arithmetic_v<T>, "clip type is not arithmetic type");
  bool isNegative{false};
  if (std::is_floating_point_v<T> && data < 0) {
    data = -data;
    isNegative = true;
  }

  double value{data};
  if (is_greater(std::numeric_limits<T>::min(), data)) {
    value = std::numeric_limits<T>::min();
  }

  if (is_greater(data, std::numeric_limits<T>::max())) {
    value = std::numeric_limits<T>::max();
  }

  if (isNegative) {
    value = -value;
  }

  return static_cast<T>(value);
}

template <typename _TpIn, typename _TpOut>
_TpOut quantify(_TpIn const& data, std::int32_t const& nBit,
                float const& offset) {
  static_assert(std::is_integral_v<_TpOut>);
  constexpr std::int32_t maxBits{32};

  std::int64_t const& scaleValue{1 << std::min(nBit, maxBits)};

  double const& value{
      std::floor(static_cast<double>(data * scaleValue) + offset)};

  return clip<_TpOut>(value);
}

}  // namespace perception
}  // namespace autodrive

#endif  // !UTILITY_H_
